/*----------------------------------------------------------------------------*/
/* Ali
/* Linessssss.java
/* Methods used with the cRIO
/*----------------------------------------------------------------------------*/

package logomotion_2011;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;

public class Line
{
    
    static boolean foundLine  = false;
    static boolean hitEnd     = false;
    static boolean clawOpened = false;


    public static boolean hitTheY = false;



    
    final static boolean on_line  = true;
    final static boolean off_line = false;

    final static double fast    = +.5;    // speed of fast movement
    final static double normal  = +.4;  // speed of normal movemen
    final static double slow  = +.2;  // speed of normal movement

    final static double reverse = -.3;    // speed of slow movement

    static double leftDrive;   // left victor value
    static double rightDrive;  // right victor value

    //Line Sensors
    public static DigitalInput sen_1 = new DigitalInput(14);
    public static DigitalInput sen_2 = new DigitalInput(11);
    public static DigitalInput sen_3 = new DigitalInput(13);
    public static DigitalInput sen_4 = new DigitalInput(12);
    
    static boolean lSensorOutCurrent;
    static boolean lSensorInCurrent;
    static boolean rSensorInCurrent;
    static boolean rSensorOutCurrent;

    static boolean lSensorOutPrev;
    static boolean lSensorInPrev;
    static boolean rSensorInPrev;
    static boolean rSensorOutPrev;

    static boolean lSensorOut;
    static boolean lSensorIn;
    static boolean rSensorIn;
    static boolean rSensorOut;
    
    //resets all vars, needs to be called after each run
    public static void Auton_Reset() {
        foundLine   = false;    
        clawOpened  = false;
        hitEnd      = false;
        hitTheY     = false;
    }
    
    //loop this method to update sensors
    public static void UpdateSensors() {
        lSensorOutCurrent = sen_1.get();
        lSensorInCurrent  = sen_2.get();
        rSensorInCurrent  = sen_3.get();
        rSensorOutCurrent = sen_4.get();
    }



    public static void DropArm()
    {
       hitEnd = true; //kills line following

       Drive_Controls.Stop();

       Drive_Controls.Drive(.5, -.5);
       Claw.arm_pickup.set(-.4); //lowers and drops tube

       Timer.delay(.5);
       Drive_Controls.Stop();

       Claw.arm_pickup.set(0);
       Timer.delay(.5);

       Claw.Claw_Close();

       Tower.Lift_Down();

       Drive_Controls.Drive(.3, -.3); //backs up
       Timer.delay(3);

       Drive_Controls.Stop(); //kill kill kill, end of Auton
    }
    
    
    //For the stright line only
    public static void Auton_Line_Follow()
    {
        UpdateSensors();
        
        //opens the claw to grab the uber at the beginning
        if (!clawOpened)
        {
            clawOpened = true;
            Claw.Claw_Open();
        }
        
        //if high peg mode is enabled, raises the lift
        if (!DriverStation_IO.getDigital(Constants.auton_mode_tower_up))
            Tower.Lift_Up();
        
        //at the T, drops tube off then backs up
        if ((lSensorInCurrent && lSensorOutCurrent && rSensorInCurrent && rSensorOutCurrent)) // if 4 sensors hit
        {

           /* if(!DriverStation_IO.getDigital(Constants.auton_mode_y_yes))
            {

                if(hitTheY == false)
                {
                    hitTheY = true; // hit the y and time to turn
                    Drive_Controls.Stop();

                    while(lSensorOutCurrent == false)
                    {
                        UpdateSensors();

                        Drive_Controls.Drive(.3, 0);

                    }
                    Drive_Controls.Stop();
                    Drive_Controls.Drive(0, -.3);
                    Timer.delay(.3);
                    Drive_Controls.Stop();


                }
                else
                {
                    DropArm();
                }
            }
            else
            {
           DropArm();
            } */

        }  
        else if(!hitEnd)
        {
            // check if any of the sensors are on the line
            if (((!lSensorInCurrent) && (!rSensorInCurrent)) && ((!lSensorOutCurrent) && (!rSensorOutCurrent)))
            {
                // set course to go in a straight line (same speed for both left and right drive)
                leftDrive = normal;
                rightDrive = normal;
            } 
            
            else // there is a sensor on the line
            {
                if ((lSensorInCurrent) || (rSensorInCurrent)) // check if it is a inner sensor
                {
                    if (lSensorInCurrent) // if it is the left inner sensor
                    {
                        // set the course for a slow adjustment to the left
                        leftDrive = slow;
                        rightDrive = normal;
                    } else // right sensor touching
                    {
                        // slow adjustment to the right
                        leftDrive = normal;
                        rightDrive = slow;
                    }

                }
                else // outer sensor
                {
                    if (lSensorOutCurrent) // outer left sensor (needs radical change in direction(fast))
                    {
                        // fast change to the left
                        leftDrive = reverse;
                        rightDrive = fast;
                    }

                    else {
                        // fast change to the right
                        leftDrive = fast;
                        rightDrive = reverse;
                    }
                }
            }
            Drive_Controls.Drive(-leftDrive, rightDrive); // give thes values to the MC
        }
    }
}
